/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <iostream>
#include <opencv/cv.h>

#include "error/cerror.h"
#include "vision/cimagefromfile.h"
#include "vision/cimagecamthread.h"
#include "vision/cpointmaxcolor.h"
#include "vision/ccamtransform.h"
#include "vision/cpointcirclecenter.h"
#include "vision/cpointmatch.h"
#include "vision/cpointbackhist.h"
#include "katanavision/ckatanawitheye.h"
#include "katanavision/katanafuncs.h"
#include "vision/cimageobject.h"
#include "vision/cobjectpyrsegmentation.h"
#include "vision/cimagedvgrab.h"

using namespace std;
using namespace Katana;

bool CmpAreas(const CImageObject * o1, const CImageObject * o2) {
	return o1->Area() > o2->Area();
};


void RemoveFalseObjects(ObjectList& objs, const CvSize& size, double border, double max_compactness) {
	SRect frame(int(border*size.width), int(border*size.height), int(size.width*(1-border)), int(size.height*(1-border)));
	CvScalar c;
	double sum;
	
	ObjectList::iterator tmp;
	for(ObjectList::iterator obj = objs.begin();  obj != objs.end();) {
		if (! (*obj)->GetRect().IsInside(frame) || (*obj)->Compactness() > max_compactness) { 
			tmp = obj++; 
			delete *tmp;
			objs.erase(tmp);
			continue;
		}
		c = (*obj)->AvgColor();
		sum = (c.val[0] + c.val[1] + c.val[2])/3;
		for(int i = 0; i < 3; i++) c.val[i] = fabs(c.val[i] - sum);
		/*if (c.val[0] < 20 && c.val[1] < 20 && c.val[2] < 20) { 
		tmp = obj++; 
		delete *tmp;
		objs.erase(tmp);
		continue;
	}*/
		obj++;
	}
	
	for(ObjectList::iterator o1 = objs.begin();  o1 != objs.end(); o1++) {
		ObjectList::iterator o2 = o1; o2++;
		while(o2 != objs.end()) {
			if ((*o1)->GetRect().Overlap((*o2)->GetRect()) && (*o1)->Merge(**o2, 2, false)) {
				tmp = o2++;
				delete *tmp;
				objs.erase(tmp);
			}
			else o2++;
		}
	}
	
	objs.sort(CmpAreas);
}

void ShowObjects(IplImage * img, ObjectList& objs, int waittime) {
	cvNamedWindow("objs", 1);
	IplImage * oimage = cvCloneImage(img);
	for(ObjectList::iterator i = objs.begin() ; i != objs.end(); i++) (*i)->Draw(oimage);
	cvShowImage("objs", oimage); 
	cvWaitKey(waittime);
	cvReleaseImage(&oimage);
}

//4 : TOWER CONSTRUCTION WITH EYES
int main( int argc, char *argv[] )
{	
	try {
		CImageCamThread cam(0, 50);
		// object detector
		CObjectPyrSegmentation det;
		det.SetSegThereholds(50, 30);
		det.SetLevel(3);
		det.SetArea(0.001, 0.027, 0.1);
		
		ObjectList objs;
		// create katana with eye
		TCdlCOMDesc ccd = {0, 57600, 8, 'N', 1, 300, 0};
		CCdlCOM device(ccd);
		CCplSerialCRC protocol;
		protocol.init(&device);
		Katana::CKatanaSensing katana;
		katana.create ( "katana5M180.cfg", &protocol );
		
		katana.unBlock();
		katana.moveRobotTo(0, 400, 0, 0, 0, 0, true, 5000);
		katana.UpdateSensors();
		katana.movDegrees(M_GRP, 70, true);
		katana.CalculateNoObstacleValues(10);
		katana.movDegrees(M_GRP, 5, true);
		katana.UpdateSensors();
		katana.UpdateSensors();
		
		IplImage * endarm_sample = cvLoadImage("pattern.jpg");
		if (!endarm_sample) throw Errors::CError("No pattern");
		CPointScaleMatch endarm(endarm_sample, 0.5, 2.0, 4, CPointNorm::NO_NORM);
		
		CCamTransform camtr("calibdata.xml");
		camtr.LoadTransform("transform.xml");
		Katana::CKatanaWithEye arm(katana, cam, endarm, camtr);	
	
		// Start of main algorthm
		cv::Point3f tower(300, 200, - 176); // Location of tower
		SRectF tower_rect(tower.x - 50, tower.y - 50, tower.x + 50, tower.y + 50); // tower rect
		double safeheight = 0; 
		double minz = -176;
		HPlane3f tableplane(0.0f, 0.0f, 1.0f, 186.0f);
		cv::Point2f iobj;
		cv::Point3f obj;
		double object_height;
		double object_sensor_gap = 20;
		
		cam.GrabFrame();
		det.DetectObjects(cam.Img(), objs);
		RemoveFalseObjects(objs, cvGetSize(cam.Img()), 0.1, 100);
		
		ShowObjects(cam.Img(), objs, 2000);

		for(ObjectList::iterator i = objs.begin() ; i != objs.end(); i++) {
			iobj = (*i)->CenterOfMass();
			cout << "Image point " << iobj << "." << endl;
			obj = camtr.ReverseProjection(iobj, tableplane);
			cout << "Try to find object at " << obj << "." << endl;
			obj.z = safeheight;
			do {
				katana.movDegrees(M_GRP, 5, true);
				object_height = FindObjectUnderGripper(katana, obj, safeheight - minz, 10, 5000) - object_sensor_gap;
				if (object_height < 0) {
					cout << "No object at " << obj << "." << endl;
					continue;
				}	
				cout << "Try to grab object at " << obj << ". Its height is " << object_height << "." << endl;
				
				//GrabVolObject(katana, object_sensor_gap + 30, 80, 18, 10, 5000);
				//GrabCrash(katana, object_sensor_gap + 30, 80, 5000) ;
				GrabForce(katana, object_sensor_gap + 30, 60, 20, 10000);
				
				if (katana.getMotorEncoders(M_GRP, true) < 2000) {
					cout << "Nothing is grabbed." << endl;
					continue;
				}
				katana.moveRobotTo(obj.x, obj.y, safeheight, 0, 0, 0, true, 5000);
			} while(!(katana.IsObstacle(S_IN_RIGHT_FRONT) && katana.IsObstacle(S_IN_LEFT_FRONT)));
			
			cout << "Try to build at " << tower << "." << endl;
			katana.moveRobotTo(tower.x, tower.y, safeheight, 0, 0, 0, true, 5000);
			tower.z += + object_height;
			katana.moveRobotTo(tower.x, tower.y, tower.z - 25, 0, 0, 0, true, 5000);
			katana.incDegrees(M_GRP, 15, true);
			katana.moveRobotTo(tower.x, tower.y, safeheight, 0, 0, 0, true, 5000);
		}
	}
	catch ( Exception &e ) {
		std::cout << "ERROR: " << e.message() << std::endl;
	}
	catch ( Errors::CError& e ) {
		e.Report(std::cout);
	}
}
